
#include <bur/plctypes.h>
#include "math.h"

#ifdef _DEFAULT_INCLUDES
	#include <AsDefault.h>
#endif





void _INIT ProgramInit(void)
{

	// read task-class cycletime
	RTInfo_0.enable = 1;
	RTInfo(&RTInfo_0);
	TCCycleTime= (REAL)RTInfo_0.cycle_time / 1000000.0;

	// init test cam
	CamAssignEnable	= 0;
	
	CamMasterSpeed	= 60;
	CamMasterFactor	= 1.0;
	CamMasterOffset	= 0.0;
	CamSlaveFactor	= 0.370;
	CamSlaveOffset	= 0.630;
	CamSlavePositionAssignStart	= 0.15;
	CamSlavePositionAssignLock	= 0.16;
	CamSlavePositionAssignEnd	= 1.2;
	for(k=1;k<=10;k++)
	{
		Cam[k].MasterPosition = 360.0/10.0*k;
	}
	
}





void _CYCLIC ProgramCyclic(void)
{

	// 10Hz sin-signal for test CommissioningCockpit
	TestSignalCnt += (360.0*TCCycleTime*10.0);
	if(TestSignalCnt >= 360.0)
		TestSignalCnt -= 360.0;
	
	gTestSignalSinus10Hz = sin(TestSignalCnt/360.0*2*3.141592);
	gTestSignalPuls10Hz = (gTestSignalSinus10Hz > 0);

	
	
	
	
	///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	// CAM - has to be called in same #TC as MC_BR_MoveCyclicPosition

	// test cam - carousel with 10 stations (tan)
	for(k=1;k<=10;k++)
	{
		Cam[k].MasterPosition += CamMasterSpeed * TCCycleTime;
		if(Cam[k].MasterPosition >= 360.0)
			Cam[k].MasterPosition -= 360.0;

		Cam[k].SlavePosition = CamSlaveOffset + CamSlaveFactor*tan((Cam[k].MasterPosition*CamMasterFactor + CamMasterOffset)/360.0*2*3.141592);
		
		// set assign flags
		if((Cam[k].MasterPosition > 300) || (Cam[k].MasterPosition < 60))
		{
			if(Cam[k].SlavePosition > CamSlavePositionAssignEnd)
			{
				Cam[k].ReadyForShuttle	= 0;
				Cam[k].ShuttleAssigned	= 0;
			}
			else if(Cam[k].SlavePosition > CamSlavePositionAssignLock)
			{
				Cam[k].ReadyForShuttle	= 0;
			}
			else if(Cam[k].SlavePosition > CamSlavePositionAssignStart)
			{
				Cam[k].ReadyForShuttle	= 1;
			}
			else
			{
				Cam[k].ReadyForShuttle	= 0;
				Cam[k].ShuttleAssigned	= 0;
			}
		}
		else
		{
			Cam[k].ReadyForShuttle	= 0;
			Cam[k].ShuttleAssigned	= 0;
		}

			
	}
	
	if(CamAssignEnable)
	{
		// search for free master and assign it to shuttle
		for(i=1;i<=MAX_NUM_SHUTTLES;i++)
		{
			if(gAssembly.Shuttle[i].Valid)
			{
				if(gAssembly.Shuttle[i].CyclicSetRequest == 1)
				{
					for(k=1;k<=10;k++)
					{
						if((Cam[k].ReadyForShuttle != 0) && (Cam[k].ShuttleAssigned == 0))
						{
							Cam[k].ReadyForShuttle	= 0;
							Cam[k].ShuttleAssigned	= i;
							gAssembly.Shuttle[i].CyclicSetRelease	= 1;
							break;
						}
					}
				}
			}	
		}
		// cyclic copy assigned master-setposition to shuttle
		for(k=1;k<=10;k++)
		{
			if(Cam[k].ShuttleAssigned != 0)
			{
				gAssembly.Shuttle[Cam[k].ShuttleAssigned].CyclicSetPosition = Cam[k].SlavePosition;
			}
		}
	}
	
	
	
	
	
	
	
}







void _EXIT ProgramExit(void)
{
}

